#include "Analyser.h"

const SearchTime cEPS = 1e-6;
Analyser::Analyser()
{
	m_sTimeBegin = -1.0f;
	m_sTimeEnd = -1.0f;
	m_sTeamSide = Team_NotSpecified;
	m_sObjName.clear();
	m_sSLTVec.clear();
	m_AgentNodeVec.clear();
}
Analyser::~Analyser()
{

}
void Analyser::SetSearchTime( SearchTime time )
{
	m_sTimeBegin = time;
	m_sTimeEnd = time;
}
void Analyser::SetSearchTimeInterval( SearchTime bTime, SearchTime eTime )
{
	m_sTimeBegin = bTime;
	m_sTimeEnd = eTime;
}
void Analyser::SetSearchTeamSide( SearchTeamSide isSearchLeft )
{
	m_sTeamSide = isSearchLeft;
}
void Analyser::SetSearchObjName( const std::string& name )
{
	m_sObjName = name;
}
bool Analyser::SearchInfo( WorldModelNode* wmNode )
{
	if( !TimeOk(wmNode) )
	{
		return false;
	}

	if( m_sObjName.empty() )
	{
		std::cerr<< "(Analyser) Error: no specified obj\n";
		return false;
	}
	
	m_sSearchedTime.push_back( wmNode->Time() );
	unsigned int slt_size = m_sSLTVec.size();
	GetObjName( wmNode );

	if( slt_size == m_sSLTVec.size() )
	{	// No resulg
		m_sSearchedTime.pop_back();
	}
	PrintSearchedSLT();

	return true;
}
bool Analyser::SearchInfo( WorldModelNode* wmNode, float* pos )
{
	if( !TimeOk(wmNode) )
	{
		return false;
	}

	if( m_sObjName.empty() )
	{
		std::cerr<< "(Analyser) Error: no specified obj\n";
		return false;
	}

	m_sSearchedTime.push_back( wmNode->Time() );
	unsigned int slt_size = m_sSLTVec.size();
	GetObjName( wmNode );

	if( slt_size == m_sSLTVec.size() )
	{	// No result
		m_sSearchedTime.pop_back();
		return false;
	}

	for( int i = 0; i < 16; i ++ )
	{
		pos[i] = m_sSLTVec[0].slt[i];
	}

	return true;
}
bool Analyser::SearchInfo( WorldModelNode* wmNode, std::vector< ObjPosSLT >& posVec )
{
	if( !TimeOk(wmNode) )
	{
		return false;
	}

	if( m_sObjName.empty() )
	{
		std::cerr<< "(Analyser) Error: no specified obj\n";
		return false;
	}

	m_sSearchedTime.push_back( wmNode->Time() );
	unsigned int slt_size = m_sSLTVec.size();
	GetObjName( wmNode );

	if( slt_size == m_sSLTVec.size() )
	{	// No result
		m_sSearchedTime.pop_back();
		return false;
	}
	
	for( unsigned int i = 0; i < m_sSLTVec.size(); i ++ )
	{
		ObjPosSLT pos;
		for( int j = 0; j < 16; j ++ )
		{
			pos.slt[j] = m_sSLTVec[i].slt[j];
		}

		posVec.push_back( pos );
	}

	return true;
}
bool Analyser::SearchInfo( std::vector<WorldModelNode*> &wmNodeVec )
{
	for( unsigned int i = 0; i < wmNodeVec.size(); i ++ )
	{
		if( !TimeOk(wmNodeVec[i]) )
		{
			continue;
		}
		else
		{
			SearchInfo( wmNodeVec[i] );
		}
	}

	return false;
}
bool Analyser::AnalyseAgentInfo( WorldModelNode* wmNode, std::vector<AgentInfo>& agentInfoVec )
{
 	RenderNode* rn = wmNode->GetRenderNode();
	GetAgentNode( rn );

	for( unsigned int i = 0; i < m_AgentNodeVec.size(); i ++ )
	{
		RenderNode* rn = m_AgentNodeVec[i];

		if( rn->GetFather() != NULL )
 		{
 			if( rn->GetFather()->GetFather() != NULL )
 			{
 				if( m_AgentNodeVec[i]->GetFather() == NULL )
 				{
 					continue;
 				}

				RenderNode* bodyrn = GetAgentBodyNode(rn->GetFather()->GetFather());
				if( bodyrn == NULL )
				{
					continue;
				}

 				AgentInfo agentInfo;
 				agentInfo.name = GetAgentNum( rn->GetFather()->GetFather() );
 				agentInfo.pos = bodyrn->GetFather()->GetFather()->GetTransform();
 				agentInfoVec.push_back( agentInfo );
 			}
 		}
	}
 
 	return true;
}
void Analyser::GetAgentNode( RenderNode* rn )
{
 	const std::string name = "naohead";

	if( rn->GetObj().empty() )
	{
		for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
			rnit != rn->GetChild().end(); rnit ++ )
		{
			RenderNode* chrn = *rnit;
			if( FindName( chrn->GetObj(), name ) )
			{
				m_AgentNodeVec.push_back( rn );
				return;
			}
		}
	}

	for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
		rnit != rn->GetChild().end(); rnit ++ )
	{
		RenderNode* chrn = *rnit;
		GetAgentNode( chrn );
	}
}
RenderNode* Analyser::GetAgentBodyNode( RenderNode* rn )
{
	const std::string name = "naobody";

	if( rn->GetObj().empty() )
	{
		for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
			rnit != rn->GetChild().end(); rnit ++ )
		{
			RenderNode* chrn = *rnit;
			if( FindName( chrn->GetObj(), name ) )
			{
				return chrn;
			}
		}
	}

	for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
		rnit != rn->GetChild().end(); rnit ++ )
	{
		RenderNode* chrn = *rnit;
		return GetAgentBodyNode( chrn );
	}

	return NULL;
}
std::string Analyser::GetAgentNum( RenderNode* rn )
{
	const std::string number_idx = "Num";
	const std::string side_left_idx = "Left";
	const std::string side_right_idx = "Right";
	std::string number;
	std::string side;

	RenderNode* bodyNode = GetAgentBodyNode( rn );
	if( bodyNode == NULL )
	{
		return std::string("unknown");
	}

	std::vector<std::string> resetMat = bodyNode->GetResetMaterials();
	for( unsigned int i = 0; i < resetMat.size(); i ++ )
	{
		if( FindName( resetMat[i], number_idx ) )
		{
			number = resetMat[i];
		}

		if( FindName( resetMat[i], side_left_idx ) ||
			FindName( resetMat[i], side_right_idx ) )
		{
			side = resetMat[i];
		}
	}

	return GetAgentNumSide(number,side);
}
std::string Analyser::GetAgentNumSide( const std::string& num, const std::string& side )
{
	const std::string side_no_idx = "Team";
	const std::string side_left_idx = "Left";
	const std::string side_right_idx = "Right";
	std::string numside;
	if( FindName(side, side_no_idx) )
	{
		numside = side_no_idx;
	}
	else if( FindName(side, side_left_idx) )
	{
		numside = side_left_idx;
	}
	else if( FindName(side, side_right_idx) )
	{
		numside = side_right_idx;
	}

	numside.append( "_" );

	for( int i = 0; i < 10; i ++ )
	{
		char idx[10];
		itoa( i, idx, 10 );
		if( FindName(num, std::string(idx)) )
		{
			numside.append( std::string(idx) );
		}
	}

	if( numside == "_" )
	{
		numside = std::string("unknow");
	}
	return numside;
}
bool Analyser::TimeOk( WorldModelNode* wmNode )
{
	if( m_sTimeBegin < 0.0f && m_sTimeEnd < 0.0f )
	{
		return true;
	}
	else if( wmNode->Time() > m_sTimeBegin - cEPS &&  wmNode->Time() < m_sTimeEnd + cEPS )
	{
		return true;
	}

	return false;
}
void Analyser::GetObjName( WorldModelNode* wmNode )
{
	GetObjName( wmNode->GetRenderNode(), m_sObjName );
}
void Analyser::GetObjName( RenderNode* rn, const std::string& name )
{
	if( rn->GetObj().empty() )
	{
		for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
			rnit != rn->GetChild().end(); rnit ++ )
		{
			RenderNode* chrn = *rnit;
			if( FindName( chrn->GetObj(), name ) )
			{
				SearchSLT s;
				for( int i = 0; i < 16; i ++ )
				{
					s.slt[i] = rn->GetTransform()[i];
				}
				m_sSearchedName.push_back( chrn->GetObj() );
				m_sSLTVec.push_back( s );
				return;
			}
		}
	}

	for( std::vector<RenderNode*>::iterator rnit = rn->GetChild().begin();
		rnit != rn->GetChild().end(); rnit ++ )
	{
		RenderNode* chrn = *rnit;
		GetObjName( chrn, name );
	}
}	
void Analyser::PrintSearchedSLT()
{
	std::cout<< "\nSearch for " << m_sObjName << "\n\n";
	std::cout<< "time: " << m_sSearchedTime[0] << "\n";
	for( unsigned int i = 0; i < m_sSLTVec.size(); i ++ )
	{
		std::cout<< m_sSearchedName[i] << "\n";
		for( unsigned int j = 0; j < 16; j ++ )
		{
			std::cout<< m_sSLTVec[i].slt[j] << " ";
			if( j % 4 == 3 )
			{
				std::cout<< "\n";
			}
		}
		std::cout<< "\n";
	}
}
bool Analyser::FindName( const std::string& src_str, const std::string& trg_str )
{
	if( src_str.find(trg_str) != std::string::npos )
	{
		return true;
	}

	return false;
}